#include "pathfinding.h"

mW_list * mW_pathfinding_find_path (mW_map * map, int start_x, int start_y, int end_x, int end_y)
{
	
	mW_minheap * openset;
	mW_list * closedset;
	mW_list * path;
	
	mW_pathfinding_point * current_point;
	
	
	
	if ((start_x < 0) || (start_x >= map->width) || (start_y < 0) || (start_y >= map->height))
		return NULL;
	
	if ((end_x < 0) || (end_x >= map->width) || (end_y < 0) || (end_y >= map->height))
		return NULL;
	
	
	
	openset = mW_minheap_create(mW_pathfinding_compare);
	closedset = mW_list_create();
	path = NULL;
	
	mW_minheap_insert(openset, mW_pathfinding_point_create(start_x, start_y, 0, 0, NULL));
	
	while (mW_minheap_get_size(openset) > 0)
	{
		
		current_point = mW_pathfinding_point_copy(mW_minheap_get_top(openset));
		if (current_point == NULL)
			mW_error("Pathfinding error, minheap returned null");
		mW_minheap_delete_top(openset);
		mW_list_insert(closedset, current_point);
		
		
		// if we reached the end
		if ((current_point->x == end_x) && (current_point->y == end_y))
		{
			path = mW_list_create();
			while (current_point != NULL)
			{
				mW_list_insert_first(path, mW_pathfinding_point_copy(current_point));
				current_point = current_point->parent_point;
			}
			break;
		}

		
		// check left
		mW_pathfinding_neighbor (map,
		                         current_point->x - 1,
		                         current_point->y,
		                         end_x,
		                         end_y,
		                         current_point,
		                         openset,
		                         closedset);
		
		// check right
		mW_pathfinding_neighbor (map,
		                         current_point->x + 1,
		                         current_point->y,
		                         end_x,
		                         end_y,
		                         current_point,
		                         openset,
		                          closedset);
		
		// check down
		mW_pathfinding_neighbor (map,
		                         current_point->x,
		                         current_point->y + 1,
		                         end_x,
		                         end_y,
		                         current_point,
		                         openset,
		                         closedset);
		
		// check up
		mW_pathfinding_neighbor (map,
		                         current_point->x,
		                         current_point->y - 1,
		                         end_x,
		                         end_y,
		                         current_point,
		                         openset,
		                         closedset);
		                             
	}
	
	// free openset
	mW_minheap_destroy(openset);
	// free closedset
	mW_list_destroy(closedset);
	
	return path;
	
}



int mW_pathfinding_neighbor (mW_map * map, int x, int y, int end_x, int end_y, mW_pathfinding_point * current_point, mW_minheap * openset, mW_list * closedset)
{
	
	mW_pathfinding_point compare_point;
	mW_pathfinding_point * neighbor_point;


	// is the neighbor point within the bounds of the map
	if ((x >= 0) && (x < map->width) && (y >= 0) && (y <= map->height))
	{
		
		// get the neighbor point from openset
		compare_point.x = x;
		compare_point.y = y;
		neighbor_point = mW_minheap_search(openset, &compare_point, mW_pathfinding_search_function);
		
		// if it does not exist in the open set
		if (neighbor_point == NULL)
		{
			
			// if it does not exist in the closed set
			if (mW_pathfinding_point_exists(closedset, x, y) == 0)
			{
				
				// is this point passable?
				if (mW_map_passable(map, x, y) == 1)
				{
					// create a point for this neighbor
					neighbor_point = mW_pathfinding_point_create(x,
																 y,
																 0,
																 mW_pathfinding_calculate_heuristic(x,
																									y,
																									end_x,
																									end_y),
																 current_point);
					// insert this neighbor in to the openset
					mW_minheap_insert(openset, neighbor_point);
				}
			}
			
		}
		
		// if it does exist in the open set
		else
		{
			
			// is current_point a cheaper parent to neighbor_point?
			if (neighbor_point->h - mW_PATHFINDING_MOVEMENT_COST > current_point->h)
			{
				neighbor_point->parent_point = current_point;
				neighbor_point->h = current_point->h + mW_PATHFINDING_MOVEMENT_COST;
			}
			
		}
		
	}
	
	return 1;

}



int mW_pathfinding_search_function (void * one, void * two)
{
	
	mW_pathfinding_point * left;
	mW_pathfinding_point * right;
	
	left = (mW_pathfinding_point *) one;
	right = (mW_pathfinding_point *) two;
	
	if ((left->x == right->x) && (left->y == right->y))
		return 0;
	else
		return 1;
		
}



int mW_pathfinding_compare (mW_pathfinding_point * one, mW_pathfinding_point * two)
{
	
	if (one->g + one->h < two->g + two->h)
		return -1;
	else if (one->g + one->h > two->g + two->h)
		return 1;
	else
		return 0;
		
}



int mW_pathfinding_point_exists (mW_list * set, int x, int y)
{
	
	mW_pathfinding_point * point;
	
	mW_list_reset_iterator(set, 1);
	point = mW_list_iterate(set, 1);
	while (point != NULL)
	{
		
		if ((point->x == x) && (point->y == y))
			return 1;
		point = mW_list_iterate(set, 1);
	
	}
	
	return 0;
	
}



mW_pathfinding_point * mW_pathfinding_point_from_list (mW_list * set, int x, int y)
{
	
	mW_pathfinding_point * point;
	
	mW_list_reset_iterator(set, 1);
	point = mW_list_iterate(set, 1);
	while (point != NULL)
	{
		
		if ((point->x == x) && (point->y == y))
			return point;
		point = mW_list_iterate(set, 1);
	
	}
	
	return NULL;

}



int mW_pathfinding_calculate_heuristic (int start_x, int start_y, int end_x, int end_y)
{
	
	int x;
	int y;
	
	x = end_x - start_x;
	if (x < 0)
		x *= -1;
		
	y = end_y - start_y;
	if (y < 0)
		y *= -1;
		
	return x + y;

}




mW_pathfinding_point * mW_pathfinding_point_copy (mW_pathfinding_point * source)
{
	
	mW_pathfinding_point * destination;
	destination = (mW_pathfinding_point *) malloc(sizeof(mW_pathfinding_point));
	if (destination == NULL)
		mW_error("Failed to allocate memory when copying pathfinding point");
	
	destination->x = source->x;
	destination->y = source->y;
	destination->g = source->g;
	destination->h = source->h;
	destination->parent_point = source->parent_point;
	
	return destination;
	
}



mW_pathfinding_point * mW_pathfinding_point_create (int x, int y, int g, int h, mW_pathfinding_point * parent)
{
	
	mW_pathfinding_point * pathfinding_point;
	
	pathfinding_point = (mW_pathfinding_point *) malloc(sizeof(mW_pathfinding_point));
	if (pathfinding_point == NULL)
		mW_error("Failed to allocate memory for pathfinding point\n");
	
	pathfinding_point->x = x;
	pathfinding_point->y = y;
	pathfinding_point->g = g;
	pathfinding_point->h = h;
	pathfinding_point->parent_point = parent;
	
	return pathfinding_point;
	
}
